/*
bea_u951.c
offer bea u951 lidar config and get data function 
designed by lunanting

----------------------------
2020-11-06 18:53:04
create file
----------------------------
*/

#include "bea_u951.h"
#include "../../bsp/cli_cmd.h"
#include "../../../Include/aos/kernel.h"
#include "../../bsp/xp_uart.h"
#include "stdio.h"
#include "string.h"
#include "math.h"

#define PI 3.1415926
#define BEA_key 0x90d9

const u8 cmd_short[16]={0xBE,0xA0,0x12,0x34,0x02,0x10,0x00,0x02,0x00,0x00,0x00,0x5B,0xC3,0x00,0x89,0xB1};
const u8 cmd_set_100[37]={0xBE, 0xA0, 0x12, 0x34, 0x02, 0x25, 0x00, 0x02, 0x00,
                       0x00, 0x00, 0x53, 0xC3, 0x00, 0x00, 0x00, 0x00, 0x00,
                       0x00, 0x00, 0x00, 0x64, 0x00, 0x00, 0x00, 0x00, 0x00,
                       0x00, 0x00, 0x30, 0x2A, 0x00, 0x00, 0x00, 0x00, 0x6e, 0xa1
                       };
const u8 cmd_set_400[37]={0XBE,0XA0,0X12,0X34,0X02,0X25,0X00,0X02,0X00,0X00,0X00,0X53,
                          0XC3,0X00,0X00,0X00,0X01,0X00,0X00,0X00,0X00,0X90,0X01,0X00,
                          0X00,0X00,0X00,0X00,0X00,0X30,0X2A,0X00,0X00,0X00,0X00,0x8B,0xE0};
const u8 cmd_store[15]={0xBE, 0xA0, 0x12, 0x34, 0x02, 0x0F, 0x00, 0x02, 0x00, 0x00, 0x00, 0x55, 0xC3, 0x6D, 0x27};

static int xp_bea_short_mode_set(BEA_class *bea);
static int xp_bea_100_point_set(BEA_class *bea);
static int xp_bea_store_config(BEA_class *bea);
static u16 xp_bea_crc(u8 *data,int len);
static void xp_lidar_H_W_analysis(BEA_class *bea,u8 *data,u8 offset,u8 len);
void GaussFilter(u8* data);


/***********************************init bea lidar*********************************/
/*
init lidar
param:
    bea:bea obj
    port:uart port;
    speed: uart speed
return:
    0=success
*/
int xp_bea_init(BEA_class *bea,u8 port,u32 speed){
    int sta=0;
    char buf[30];

    bea->port=port;
    bea->speed=speed;

    bea->uart.port=port;
    bea->uart.config.baud_rate=speed;
    bea->uart.config.data_width=DATA_WIDTH_8BIT;
    bea->uart.config.parity=NO_PARITY;
    bea->uart.config.stop_bits=STOP_BITS_1;
    bea->uart.config.flow_control=FLOW_CONTROL_DISABLED;
    bea->uart.config.mode=MODE_TX_RX;

    sta=hal_uart_init(&bea->uart);
    if(sta!=0){
        perr("bea init fail~ uart init fail");
        return -1;
    }
    aos_msleep(10);
    //short mode
    xp_bea_short_mode_set(bea);
    xp_uart_rec_clear(&bea->uart);
    //config 100 point
    xp_bea_100_point_set(bea);
    //save config
    xp_bea_store_config(bea);

    sprintf(buf,"bea data,port=%d",port);
    void xp_bea_get_data_thread(void *arg);
    if(aos_task_new(buf,xp_bea_get_data_thread,bea,2100)!=0){
        perr("bea init fail~ create get data thread fail~");
        return -2;
    }
    println("bea lidar init success~");
    return 0;
}

/*
bea send fail log
*/
static void xp_bea_send_fail_log(BEA_class *bea){
    perr("bea %d send cmd fail~",bea->port);
}

/*
bea rec fail log
*/
static void xp_bea_rec_fail_log(BEA_class *bea){
    perr("bea %d rec fail~",bea->port);
}

/********************************cmd send***********************************/
/*
set short mode
return:
    0=success
*/
static int xp_bea_short_mode_set(BEA_class *bea){
    int sta=0;

    sta=hal_uart_send(&bea->uart,cmd_short,sizeof(cmd_short),10);
    if(sta!=0){
        xp_bea_send_fail_log(bea);
        return -1;
    }
    sta=hal_uart_recv_II(&bea->uart,bea->data,100,&bea->rec_len,100);
    if(sta!=0||bea->rec_len==100){
        xp_bea_rec_fail_log(bea);
        return -2;
    }
    return 0;
}


/*
set 100 point mode
return:
    0=success
*/
static int xp_bea_100_point_set(BEA_class *bea){
    int sta=0;

    sta=hal_uart_send(&bea->uart,cmd_set_100,sizeof(cmd_set_100),10);
    if(sta!=0){
        xp_bea_send_fail_log(bea);
        return -1;
    }
    sta=hal_uart_recv_II(&bea->uart,bea->data,100,&bea->rec_len,100);
    if(sta!=0||bea->rec_len==100){
        xp_bea_rec_fail_log(bea);
        return -2;
    }
    return 0;
}

/*
save config
return:
    0=success
*/
static int xp_bea_store_config(BEA_class *bea){
    int sta=0;

    sta=hal_uart_send(&bea->uart,cmd_store,sizeof(cmd_store),10);
    if(sta!=0){
        xp_bea_send_fail_log(bea);
        return -1;
    }
    sta=hal_uart_recv_II(&bea->uart,bea->data,100,&bea->rec_len,100);
    if(sta!=0||bea->rec_len==100){
        xp_bea_rec_fail_log(bea);
        return -2;
    }
    return 0;
}


/***********************************get lidar data********************************/
/*
get lidar data thread
param:
    lidar object
*/
void xp_bea_get_data_thread(void *arg){
    BEA_class *bea=(BEA_class *)arg;
    u8 buf[200];
    u8 points;
    int sta=0;

    while(1){
		xp_uart_rec_clear(&bea->uart);
        sta=hal_uart_send(&bea->uart,cmd_short,sizeof(cmd_short),10);
        if(sta!=0){
            xp_bea_send_fail_log(bea);
            aos_msleep(100);
            continue;
        }
        sta=hal_uart_recv_II(&bea->uart,bea->data,215,&bea->rec_len,100);
        if(sta!=0||bea->rec_len!=215){
            if(bea->online==1){
                bea->online=0;
                xp_bea_rec_fail_log(bea);
                perr(" bea lidar offline~");
            }
            aos_msleep(100);
            continue;
        }
        if(bea->online!=1){
            println("bea lidar online~");
            bea->online=1;
        }
        //deal lidar data
        points = (bea->data[6] << 8) | bea->data[5] - 15;
        memcpy(buf,&bea->data[13],200);
        GaussFilter(buf);
        // xp_lidar_H_W_analysis(bea,buf,50,40);
        xp_lidar_H_W_analysis(bea,buf,bea->offset,bea->use_len);

        aos_msleep(500);
    }
}



/**************************************analysis lidar data*************************/
/*
analysis hight and width by lidar data
分析雷达数据的高度和宽度
param:
    bea:lidar object
	data:lidar data
    offset:offset point
	len:use point number
*/
static void xp_lidar_H_W_analysis(BEA_class *bea,u8 *data,u8 offset,u8 len){
	u16 dis=0,minW=1500;
	int maxH=-500;
	float H=0.0,W=0.0;
	double angle,C,S,A;

    int l=100-offset;
    if(len>l||len==0)len=l;
	for(u8 i=0;i<len;i++){
		memcpy(&dis,data+offset*2+i*2,2);
		// println("point=%d,	dis=%d",i,dis);
		if(dis<2000&&dis>100){
			if(i<(50+BEA_dir-offset)){
				if(dis<2000&&dis>100){
					angle=0.54+(((49+BEA_dir-offset)-i)*1.08);
					A=angle*PI/180.0;
					C=cos(A);
					W=C*dis;
					S=sin(A);
					H=S*dis;
				}
			}
			else{
				if(dis<2000&&dis>100){
					angle=0.54+((i-(50-BEA_dir-offset))*1.08);
					A=angle*PI/180.0;
					C=cos(A);
					W=C*dis;
					S=sin(A);
					H=-1*S*dis;
				}
			}
			if(W<minW){
				minW=(int)W;
				// minDis=minW;
			}
			if(H>maxH){
				maxH=(int)H;
				// maxHigh=maxH;
			}
			// println("point=%d,	dis=%d,	hight=%0.2f,	width=%0.2f",i,dis,H,W);
		}
	}	
	//println("\r\n point num=%d,	maxH=%d,	minW=%d\r\n\r\n\r\n",len,maxH,minW);
	if((minW>=1500))minW=0;
    bea->hight_max=maxH;
    bea->dis_min=minW;
}




/*
lidar crc 16 bit
param:
    data:crc data
    len:data length
return: crc data
*/
static u16 xp_bea_crc(u8 *data,int len){
	uint16_t crc = 0;
	uint16_t i, j;
	for(i = 0; i < len; i++)
	{
		crc ^= (uint16_t)(data[i] << 8); /* move byte into MSB of 16bit CRC */
		for(j = 0; j < 8; j++)
		{
			if((crc & 0x8000) != 0){
				crc = (uint16_t)((crc << 1) ^ BEA_key);
			}else{
				crc <<= 1;
			}
		}
	}
	
	return crc;
}


/**********************debug*************************/
//bea debug
int xp_beaU951_debug(char *type,char *fun,char *param){
    u16 i=0;

    if(strcmp(type,"u951")!=0)return 0;
    if(strcmp(fun,"crc")==0){
        i=xp_bea_crc(cmd_set_400,35);
        println("crc=%02X,%02X",i&0xff,i>>8);
    }
    else{
        return 0;
    }
    return 1;
}


//end of the file
